/*
 * CameraMotionValidator.cpp
 *
 *  Created on: Mar 23, 2013
 *      Author: ohnozzy
 */
#include <math.h>
#include <fcl/math/transform.h>
#include "CameraMotionValidator.h"
#include "CameraSpace.h"
#include "flyingcamera/conversion/Conversion.h"
bool flyingcamera::base::CameraMotionValidator::checkMotion(const ompl::base::State *s1, const ompl::base::State *s2) const{
	const flyingcamera::base::CameraSpace::StateType* cs1=s1->as<flyingcamera::base::CameraSpace::StateType>();
	const flyingcamera::base::CameraSpace::StateType* cs2=s2->as<flyingcamera::base::CameraSpace::StateType>();
	if(cs1->getT()<cs2->getT()){

	  if(checkAngelChangeRate(cs1, cs2))
		 return DiscreteMotionValidator::checkMotion(s1,s2);

	}
	return false;

}
bool flyingcamera::base::CameraMotionValidator::checkAngelChangeRate(const flyingcamera::base::CameraSpace::StateType* cs1, const flyingcamera::base::CameraSpace::StateType* cs2) const{
	if(max_angle_rate>0){
	 fcl::Quaternion3f r1, r2;
	 flyingcamera::conversion::ompl2fclRotation(cs1->rotation(), r1);
	 flyingcamera::conversion::ompl2fclRotation(cs2->rotation(), r2);
	 double change=flyingcamera::AngleChangeCosValue(r1, r2);
	 return change/(cs2->getT()-cs1->getT())<max_angle_rate;
	}
	return true;
}
void flyingcamera::base::CameraMotionValidator::setChangeRateRadian(double change_angle, double duration ){
	max_angle_rate=(1-cos(change_angle))/duration;
}
bool flyingcamera::base::CameraMotionValidator::checkMotion(const ompl::base::State *s1, const ompl::base::State *s2, std::pair<ompl::base::State*, double> &lastValid) const{
	const flyingcamera::base::CameraSpace::StateType* cs1=s1->as<flyingcamera::base::CameraSpace::StateType>();
	const flyingcamera::base::CameraSpace::StateType* cs2=s2->as<flyingcamera::base::CameraSpace::StateType>();
	if(cs1->getT()<cs2->getT())
		if(checkAngelChangeRate(cs1, cs2))
	      return DiscreteMotionValidator::checkMotion(s1,s2, lastValid);
	else{
	  lastValid.second=0;

	  if (lastValid.first)
		  si_->getStateSpace()->copyState(lastValid.first,cs1);

	}
	return false;

}


